/*
 * Servos.c
 *
 *  Created on: 29/12/2012
 *      Author: Bowmer
 */


#include "inc/lm4f120h5qr.h"


void updateServos(float slewAngle, float luffAngle){
	unsigned long slewVal, luffVal;

	/* Transformations */
	slewVal = 0.5442*slewAngle*slewAngle + 345.16*slewAngle + 47728;
	luffVal = 0.0632*luffAngle*luffAngle - 355.56*luffAngle + 50688;


	/* Limit range that is sent to the PWM. Exceeding the limits causes the servo to jam. Not fun */
	if(slewVal < 0x5100)slewVal = 0x5100;
	else if(slewVal > 0x15500) slewVal = 0x15500;

	TIMER0_TAPMR_R = ((slewVal & 0xFF0000) >> 16);
	TIMER0_TAMATCHR_R = slewVal & 0xFFFF;


	/* Limit range that is sent to the PWM. Luff up is physically limited to ~+44deg */
	if(luffVal < 0x8750)luffVal = 0x8750;
	else if(luffVal > 0x15500) luffVal = 0x15500;

	TIMER0_TBPMR_R = ((luffVal & 0xFF0000) >> 16);
	TIMER0_TBMATCHR_R = luffVal & 0xFFFF;
}
